"""
batwing.py --

This file implements the batwing class.

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
"""

import vtk
from vtk.util.colors import *
import time

from numpy import *
from graphengine import axes
from graphengine import cindir
from graphengine import tube
from graphengine import cylinder
from graphengine import halfcylinder
from graphengine import cylinderhole
from graphengine import screw
from graphengine import base
from graphengine import sphere
from systems import basesystem
from systems import link
from systems import dcmotor

class Batwing( basesystem.BaseSystem):
	""" batwing system class definition"""

	#geometric parameters - mts, degrees.
	Pend1Length	= 0.3								#pendulum 1 lenght
	Pend2Length	= 0.35								#pendulum 2 lenght
	Pendradius		= 0.0075							#pendulums radius
	BaseLength	    = 0.4								#base Length
	BaseRadius 	= 0.06								#base radius
	BallRadius		= Pendradius+0.01		#ball radius

	#dynamic parameters - kg, mts.
	m             = 0.2		#ball mass
	Pend2Ine = 0.06		#pendulum 2 inertia
	Pend2fric = 0.0		#pendulum 2 friction
	grav         = 9.8       #world gravity

	#dc-motor initialization.
	dcmotor = dcmotor.DCmotor('motor1')
	dcmotor.Ka = 1.0
	dcmotor.Ta = 10.0

	#class methods
	def baseobject(self, ren):
		""" This function builds the 3d object that represents the base of this system."""

		#Base cylinder:
		#dimensions (length, radius), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		basecylinderlenght = self.BaseLength-0.05
		#Geometrical dimensions.
		basecylindergeomdim = [ basecylinderlenght, self.BaseRadius+0.035 ]
		#Position and orientation.
		basecylinderposorient = [ 0.0,  0.0, 0.0, -90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		basecylinderlightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		basecylindercolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.BaseCylinderObject	=	cylinder.Cylinder( ren, basecylindergeomdim, basecylinderposorient, basecylinderlightprop, slate_grey )

		#Screw 1 figure
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		screw1geomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		screw1posorient = [ 0.0, -(self.BaseRadius+0.025), -(basecylinderlenght+0.002), 90.0, 45.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		screw1lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		screw1colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Screw1Object	=	 screw.Screw( ren, screw1geomdim, screw1posorient, screw1lightprop, light_grey )

		#Screw 2 figure
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		screw2geomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		screw2posorient = [ 0.0, (self.BaseRadius+0.025), -(basecylinderlenght+0.002), 90.0, 45.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		screw2lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		screw2colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Screw2Object	=	 screw.Screw( ren, screw2geomdim, screw2posorient, screw2lightprop, light_grey )

		#Screw 3 figure
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		screw3geomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		screw3posorient = [ (self.BaseRadius+0.025), 0.0, -(basecylinderlenght+0.002), 90.0, 45.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		screw3lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		screw3colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Screw3Object	=	 screw.Screw( ren, screw3geomdim, screw3posorient, screw3lightprop, light_grey )

		#Screw 4 figure
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		screw4geomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		screw4posorient = [ -(self.BaseRadius+0.025), 0.0, -(basecylinderlenght+0.002), 90.0, 45.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		screw4lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		screw4colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Screw4Object	=	 screw.Screw( ren, screw4geomdim, screw4posorient, screw4lightprop, light_grey )

		#Second cylinder
		cylinder1lenght = 0.04
		#dimensions (length, radius), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		cylinder1geomdim = [ cylinder1lenght , self.BaseRadius-0.01 ]
		#Position and orientation.
		cylinder1posorient = [ 0.0,  0.0, -(basecylinderlenght), -90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cylinder1lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cylinder1colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Cylinder1Object	=	base.Base( ren, cylinder1geomdim, cylinder1posorient, cylinder1lightprop, peacock )

		#Motor cylinder
		motorlenght = 0.025
		motorradius = 0.04
		#dimensions (length, radius), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		motorgeomdim = [ motorlenght , motorradius ]
		#Position and orientation.
		motorposorient = [ 0.0,  0.0, -(basecylinderlenght), -90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		motorlightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		motorcolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.MotorObject	=	cylinder.Cylinder( ren, motorgeomdim, motorposorient, motorlightprop, peacock )


		#Base assembly
		assembly = vtk.vtkAssembly()
		assembly.AddPart( self.BaseCylinderObject.cylinderActor )
		assembly.AddPart( self.Screw1Object.screwActor )
		assembly.AddPart( self.Screw2Object.screwActor )
		assembly.AddPart( self.Screw3Object.screwActor )
		assembly.AddPart( self.Screw4Object.screwActor )
		assembly.AddPart( self.Cylinder1Object.baseActor )
		assembly.AddPart( self.MotorObject.cylinderActor )

		return assembly

	def pend1object(self, ren):
		""" This function builds the 3d object that represents the first pendulum of this system."""

		#Pendulum's  back part:
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		backgeomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius-0.003 ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		backposorient = [ 0.0,  0.0, -self.Pend1Length/2+0.0357, 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		backlightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		backcolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.BackObject	=	cylinderhole.CylinderHole( ren, backgeomdim, backposorient, backlightprop, slate_grey )

		#Pendulum's main tube:
		#dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		pole1geomdim = [ 0.0, self.Pend1Length+0.02, 0.0, self.Pendradius+0.00125 ]
		#Position and orientation.
		pole1posorient = [ 0.0,  0.0, -((self.Pend1Length/2)-0.055+0.0175), 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		pole1lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		pole1colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Pend1Object	=	tube.Tube( ren, pole1geomdim, pole1posorient, pole1lightprop, light_grey )

		#Screw figure
		#dimensions (length, height, width, radius),
		#Geometrical dimensions.
		screwgeomdim = [ 0.0, self.Pendradius+0.015, 0.0, self.Pendradius-0.005 ]
		#Position and orientation: (xpos, ypos, zpos, r,p,y ).
		screwposorient = [ 0.0,  -0.015, 0.0, 0.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		screwlightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		screwcolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.ScrewObject	=	cylinderhole.CylinderHole( ren, screwgeomdim, screwposorient, screwlightprop, slate_grey )

		#Pendulum1 assembly
		assembly = vtk.vtkAssembly()
		assembly.AddPart( self.BackObject.wheelActor )
		assembly.AddPart( self.ScrewObject.wheelActor )
		assembly.AddPart(self.Pend1Object.tubeActor)
		return assembly

	def pend2object(self, ren):
		""" This function builds the 3d object that represents the second pendulum of this system."""

		#Tube:
		#dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		pole2geomdim = [ 0.0, self.Pend2Length, 0.0, self.Pendradius+0.00125 ]
		#Position and orientation.
		pole2posorient = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		pole2lightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		pole2colorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.Pend2Object	=	tube.Tube( ren, pole2geomdim, pole2posorient, pole2lightprop, light_grey )

		#Cylinder with hole:
		#dimensions (length, height, width), pos. and orient. (xpos, ypos, zpos, r,p,y ).
		#Geometrical dimensions.
		cylholegeomdim = [ 0.0, self.Pendradius+0.015, 0.0, 0.01 ]
		#Position and orientation.
		cylholeposorient = [ 0.0,  0.001, -0.0110, 90.0, 0.0, 0.0 ]
		#Lighting properties: opacity, ambient, diffuse, specular, specularpower.
		cylholelightprop = [ 0.4, 0.5, 0.6, 1.0, 10.0 ]
		#Color properties:ambient, diffuse, specular.
		cylholecolorprop = [ ( 0.1, 0.1, 0.1 ), ( 0.1, 0.4, 0.8 ), ( 1.0, 1.0, 1.0 ) ]
		self.CylinderHoleObject	=	cylinderhole.CylinderHole( ren, cylholegeomdim, cylholeposorient, cylholelightprop, slate_grey )

		#setting the sphere geometric parameters
		pole2sphere = vtk.vtkSphereSource()
		pole2sphereMapper = vtk.vtkPolyDataMapper()
		pole2sphereMapper.SetInputConnection( pole2sphere.GetOutputPort())
		pole2sphere.SetPhiResolution(30)
		pole2sphere.SetThetaResolution(30)
		pole2sphere.SetRadius(self.BallRadius+0.005 )
		self.pole2sphereActor = vtk.vtkActor()
		self.pole2sphereActor.SetMapper(pole2sphereMapper)
		self.pole2sphereActor.AddPosition( 0, (self.Pend2Length+ self.BallRadius+0.005 ), 0 )
		self.pole2sphereActor.GetProperty().SetColor(slate_grey)

		#Pendulum2 assembly
		assembly = vtk.vtkAssembly()
		assembly.AddPart(self.Pend2Object.tubeActor)
		assembly.AddPart( self.pole2sphereActor )
		assembly.AddPart(self.CylinderHoleObject.wheelActor)

		return assembly


	def draw( self, ren ):
		"""Renderizes batwing on 3D screen"""

		#Base part of the main assembly - axes
		self.BaseAxes = axes.CreateAxes()
		#Associated 3D object.
		self.BaseObject = self.baseobject( ren )

		#baseassembly
		self.Baseassembly = vtk.vtkAssembly()
		self.Baseassembly.SetUserTransform( self.BaseTrans )
		self.Baseassembly.AddPart(self.BaseAxes)
		self.Baseassembly.AddPart(self.BaseObject)

		#Pend1 part of the assembly.
		self.Pend1Axes = axes.CreateAxes()
		#Associated 3D objects.
		self.Pend1Object = self.pend1object( ren )
		#Link 1: Pendulum 1 assembly.
		self.Link1assembly = vtk.vtkAssembly()
		self.Link1assembly.SetUserTransform( self.Link1.Trans )
		self.Link1assembly.AddPart(self.Pend1Axes)
		self.Link1assembly.AddPart(self.Pend1Object)

		#Pend2 part of the assembly
		#transformation axes
		self.Pend2Axes = axes.CreateAxes()
		#Associated 3D object.
		self.Pend2Object = self.pend2object( ren )
		#Link 2: pend2assembly.
		self.Link2assembly = vtk.vtkAssembly()
		self.Link2assembly.SetUserTransform( self.Link2.Trans )
		self.Link2assembly.AddPart(self.Pend2Axes)
		self.Link2assembly.AddPart(self.Pend2Object)

		# Create the main assembly.  Also set the origin,
		# position and orientation in space.
		self.Systemassembly = vtk.vtkAssembly()
		self.Systemassembly.AddPart(self.Baseassembly)
		self.Systemassembly.AddPart(self.Link1assembly)
		self.Systemassembly.AddPart(self.Link2assembly)

	def filldynsimulate(self):
		"""Fills 3D simulation values"""
		#DYNMODEL      m    g       r     L      I       bs
		self.SimParam[0,:]=[ self.m ,self.grav, self.Pend1Length, self.Pend2Length, self.Pend2Ine ,self.Pend2fric ]

		#DCMOTOR        Ka   Ta
		if (self.dcmotor.type == 'motor1'):
			self.SimParam[1,:]=[self.dcmotor.Ka, self.dcmotor.Ta, 0.0, 0.0, 0.0, 0.0]




